/*
 * TiTechBoard.cpp
 *
 *  Created on: Aug 3, 2009
 *      Author: naveenoid
 */

#include "TitechBoard.hpp"
#include <stdio.h>
#include <stdlib.h>
#include "./../Common/Common.hpp"

using std::vector;
using std::string;
using namespace Octopus::BoardSetup::FTD2xx;
using Octopus::Common::motorState;

namespace Octopus {

namespace BoardSetup {

TitechBoard *TitechBoard::instance = NULL;

TitechBoard *TitechBoard::getInstance()
{
	if(instance==NULL)
	{
		instance = new TitechBoard;
	}
	return(instance);
}

void TitechBoard::remove()
{
	delete(this);
}

bool TitechBoard::deviceCanCom(void) {
	return(comStatus);
}

bool TitechBoard::deviceIsOpen() {
	return(openStatus);
}

bool TitechBoard::deviceTimeOutIsSet() {
	return(timeOutStatus);
}

vector<int> TitechBoard::readAD(void) {

	FT_STATUS ftStatus;
	vector<int> temp;

	char TxBuffer[18] = ""; // Contains data to write to device
	if(openStatus!=true || comStatus !=true || timeOutStatus != true) {
		return(temp); // failure case
	}
	sprintf(TxBuffer,"READAD\n");
	if(writeData(TxBuffer)!=true)
	{
		printf("Write Failure aborting\n");
		return(temp);
	}
	else
	{
//		printf("written successfully\n");
		int RxBytes = 79;
		DWORD BytesReceived = 0;
		char RxBuffer[79];
		ftStatus = FT_Read(ftHandle,RxBuffer,RxBytes,&BytesReceived);
		if (ftStatus == FT_OK) {
//				printf("Read Completed\n");
			if (BytesReceived == (unsigned int)RxBytes) {
				// FT_Read OK
//				printf("Received : %s \n",RxBuffer);
//				returnVal = adcValueAt(RxBuffer, position);

				char *tempString;
				tempString = strtok(RxBuffer, ",");
				for(int index = 0; index < 16; index++) {
						int tempVal = atoi(tempString);
						temp.push_back(tempVal);
//						printf("AD%d:%d, ",index,tempVal);
						tempString = strtok(NULL, ",");
				}

//				for(int i= 0; i<temp.size();i++) {
//					printf("AD%d : %d ", i,temp.at(i));
//				}
//				printf("\n");

//				printf("\n");
			}
			else {
				// FT_Read Timeout
				printf("There was a timeout ");
//				printf("Received : %s with bytesReceived : %d and rxBytes : %d \n",RxBuffer,BytesReceived, RxBytes);
			}
		}
		else {
			//FT_Read Failure
			printf("Read Failed");
		}
	}

	return(temp);
}

bool TitechBoard::writeMotor(int id, int position)
{
	if(openStatus!=true || comStatus !=true || timeOutStatus != true) {
		return(false); // failure case
	}

//	printf("Command is Motor %d:%d\n", id, position);
	char TxBuffer[18] = ""; // Contains data to write to device
	id++;

	if (position<=MOTOR_POS_MAX && position>=MOTOR_POS_MIN && id>=1 && id<=MAX_NUM_OF_MOTOR)
	{
//		printf("Position : %d\n",position);

		if(id!=4)	{
			sprintf(TxBuffer,"CH%d:%d\n",id,position);
		}
		else {
			sprintf(TxBuffer,"CH%d:%d\n",id+1,position);
		}

		//2b13
//		if(id!=3)
//		{
			if(writeData((string)TxBuffer)!=true)
			{
				printf("Write Failure aborting\n");
				return(false);
			}
//		}
	}
	else
	{
		printf("Position Out of Bound\n");
		return(false);
	}
	return(true);
}

bool TitechBoard::writeMotors(vector<int> motorVector)
{
	if(openStatus!=true || comStatus !=true || timeOutStatus != true) {
		return(false); // failure case
	}

	char TxBuffer[18] = ""; // Contains data to write to device

	int position;

	for(int i=0; i< MAX_NUM_OF_MOTOR ; i++) {
		position = motorVector.at(i);

		if (position<=MOTOR_POS_MAX && position>=MOTOR_POS_MIN )
		{
			//		printf("Position : %d\n",position);

//			if(i==1 || i==3)
//			{
//				position = 1900-position;
//			}

			if(i!=3)	{
				sprintf(TxBuffer,"CH%d:%d\n",i+1,position);
			}
			else {
				sprintf(TxBuffer,"CH%d:%d\n",i+2,position);
			}
			if(writeData((string)TxBuffer)!=true)
			{
				printf("Write Failure for Motor%d; Aborting\n", i+1);
				return(false);
			}
		}
		else
		{
			printf("Position Command Out of Bound for Motor %d\n",i+1);
			return(false);
		}
	}
	return(true);
}

TitechBoard::TitechBoard(int readTimeOut, int writeTimeOut) {

	openStatus = openDevice();
	comStatus = setDeviceComCharacteristics();
	timeOutStatus = setTimeOut(readTimeOut,writeTimeOut);

	printf("New TitechBoard created\n");

}

TitechBoard::~TitechBoard() {

	printf("TitechBoard destroyed\n");
}


bool TitechBoard::openDevice() {

	printf ("starting the attempt to try and open the device \n");

//	FT_HANDLE ftHandle;
	FT_STATUS ftStatus;
	ftStatus = FT_Open(0,&ftHandle);
	if (ftStatus == FT_OK) {
	// FT_Open OK, use ftHandle to access device
		printf("Device can be opened\n");
		return true;
	}
	else {
	// FT_Open failed
		printf("Device cannot be opened\n");
	}

	return false;

}

bool TitechBoard::setDeviceComCharacteristics(void)
{
	FT_STATUS ftStatus;
	ftStatus = FT_SetBaudRate(ftHandle, 115200); // Set baud rate to 115200
	if (ftStatus == FT_OK) {

		// FT_SetBaudRate OK
		printf("Baudrate set to 115200\n");

		// Set 8 data bits, 1 stop bit and no parity
		ftStatus = FT_SetDataCharacteristics(ftHandle, FT_BITS_8, FT_STOP_BITS_1, FT_PARITY_NONE);

		if (ftStatus == FT_OK) {
		// FT_SetDataCharacteristics OK
			printf("Data Characteristics (8Data,1Stop,0Parity) have been set successfully\n");
			return true;
		}
		else {
		// FT_SetDataCharacteristics Failed
			printf("Data Characteristics setting failed\n");
		}

	}
	else {
	 // FT_SetBaudRate Failed
		printf("BaudRate setting failed\n");
	}

	return false;
}

bool TitechBoard::setTimeOut(int readTimeOut, int writeTimeOut)
{
	FT_STATUS ftStatus;
	ftStatus = FT_SetTimeouts(ftHandle,readTimeOut,writeTimeOut);
	if(ftStatus != FT_OK) {
		printf("Setting timeouts failed\n");
		return false;
	}
	return true;
}

bool TitechBoard::writeData(string TxBuffer)
{
	DWORD BytesWritten;
	FT_STATUS ftStatus = FT_Write(ftHandle, (char *)TxBuffer.c_str(), TxBuffer.size(), &BytesWritten);

	if (ftStatus == FT_OK) {
	// FT_Write OK
//		printf("%s written with %lu Bytes \n",TxBuffer, BytesWritten);
		return(true);
	}
	else {
	// FT_Write Failed
		printf("%s write failed\n", TxBuffer.c_str());
		return(false);
	}
}

}

}
